#include "CRSF.h"

uint16_t cfVal[8] = {};
uint8_t s_CF[64] = {};
uint8_t i_CF = 0;
bool cfNew = 0;

void INIT_CRSF(void)
{//初始化CRSF参数
  Serial.begin(115200);
}

void CRSF_DECODE(uint8_t in)
{//CRSF解码程序
  static uint8_t type;
  switch (i_CF)
  {
  case 0://检测帧头
    if (in == 0xC8)//帧头
    {//一旦检测到帧头
      //缓存数据清空
      for (uint8_t j = 0; j < 64; j ++)
      {
        s_CF[j] = 0;
      }
      s_CF[i_CF] = in;//帧头纳入缓冲数据
      i_CF ++;//索引上涨
    }
    break;

  case 1://数据长度
    s_CF[i_CF] = in;//数据长度纳入缓冲数据
    i_CF ++;//索引上涨
    break;

  case 2://消息类型
    switch (in)
    {//根据输入值判断消息类型
    case 0x16://遥控器数据
      type = in;
      CRSF_CRC_RX(in, 1, 0);//初始化CRSF_CRC_RX并输入数据
      s_CF[i_CF] = in;//消息类型纳入缓冲数据
      i_CF ++;//索引上涨
      break;

    case 0x14://信号质量
      type = in;
      CRSF_CRC_RX(in, 1, 0);//初始化CRSF_CRC_RX并输入数据
      s_CF[i_CF] = in;//消息类型纳入缓冲数据
      i_CF ++;
      break;

    default://其他类型数据不处理
      s_CF[1] = 0;//第1byte置零
      type = 0;//复位消息类型
      i_CF = 0;//索引号复位
      break;
    }
    break;

  default://正常数据缓存
    CRSF_CRC_RX(in, 0, 0);//CRSF_CRC_RX输入数据
    s_CF[i_CF] = in;//缓存数据
    i_CF ++;//索引号上涨
    break;

  case 25://遥控器数据的CRC位
    if (type != 0x16)
    {//只有遥控器数据类型的时候才需要判断第25byte，其他情况数据纳入缓存
      CRSF_CRC_RX(in, 0, 0);//CRSF_CRC_RX输入数据
      s_CF[i_CF] = in;//缓存数据
      i_CF ++;//索引号上涨
      break;
    }
    if (s_CF[1] != 24)
    {//如果第1byte不是24
      s_CF[1] = 0;//第1byte置零
      type = 0;//复位消息类型
      i_CF = 0;//索引号复位
      break;
    }
    if (CRSF_CRC_RX(in, 0, 1) == in)
    {//如果输入数据与CRSF_CRC_RX数据相等
      s_CF[i_CF] = in;//CRC数据纳入数组
      uint32_t tmp = ( (uint32_t)s_CF[3] | ((uint32_t)s_CF[4] << 8) );//取byte3做低，byte4做高
      cfVal[0] = tmp & 0x07FF; tmp = tmp >> 11;//0通道取前11位，取完数据右移11位
      tmp |= ((uint32_t)s_CF[5] << 5);//新数据左移5位
      cfVal[1] = tmp & 0x07FF; tmp = tmp >> 11;//1通道取前11位，取完数据右移11位
      tmp |= ((uint32_t)s_CF[6] << 2);//新数据左移2位
      tmp |= ((uint32_t)s_CF[7] << 10);//新数据左移10位
      cfVal[2] = tmp & 0x07FF; tmp = tmp >> 11;//2通道取前11位，取完数据右移11位
      tmp |= ((uint32_t)s_CF[8] << 7);//新数据左移7位
      cfVal[3] = tmp & 0x07FF; tmp = tmp >> 11;//3通道取前11位，取完数据右移11位
      tmp |= ((uint32_t)s_CF[9] << 4);//新数据左移4位
      cfVal[4] = tmp & 0x07FF; tmp = tmp >> 11;//4通道取前11位，取完数据右移11位
      tmp |= ((uint32_t)s_CF[10] << 1);//新数据左移1位
      tmp |= ((uint32_t)s_CF[11] << 9);//新数据左移9位
      cfVal[5] = tmp & 0x07FF; tmp = tmp >> 11;//5通道取前11位，取完数据右移11位
      tmp |= ((uint32_t)s_CF[12] << 6);//新数据左移6位
      cfVal[6] = tmp & 0x07FF; tmp = tmp >> 11;//6通道取前11位，取完数据右移11位
      tmp |= ((uint32_t)s_CF[13] << 3);//新数据左移3位
      cfVal[7] = tmp & 0x07FF; tmp = tmp >> 11;//7通道取前11位，取完数据右移11位
      cfNew = 1;
    }
    i_CF = 0;//索引号复位
    break;
  }
}

uint16_t CRSF_VAL(uint8_t channel)
{
  return cfVal[channel];
}

bool CRSF_UPDATED(void)
{
  bool old = cfNew;
  cfNew = 0;
  return old;
}

void CRSF_BATT_TELE(uint16_t vol, uint16_t curr, uint32_t cap, uint8_t remain)
{//CRSF电压回传数据
  uint8_t cfTX[12] = {0xC8, 0x0A, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  cfTX[3] = ((vol & 0xFF00) >> 8);
  cfTX[4] = (vol & 0xFF);
  cfTX[5] = ((curr & 0xFF00) >> 8);
  cfTX[6] = (curr & 0xFF);
  cfTX[7] = ((cap & 0xFF0000) >> 16);
  cfTX[8] = ((cap & 0xFF00) >> 8);
  cfTX[9] = (cap & 0xFF);
  cfTX[10] = remain;
  CRSF_CRC_TX(0x08, 1, 0);
  for (uint8_t i = 3; i < 11; i ++)
  {
    CRSF_CRC_TX(cfTX[i], 0, 0);
  }
  cfTX[11] = CRSF_CRC_TX(0, 0, 1);
  Serial.write(cfTX, sizeof(cfTX));
}

uint8_t CRSF_CRC_RX(uint8_t in, bool rset, bool out)
{
  static uint8_t crc;
  if (rset)
  {
     crc = 0x00;
  }
  if (out) return crc;
  crc = crc ^ in;
  for(uint8_t j = 0; j < 8; j++)
  {
    if (crc & 0b10000000)
    {
      crc = crc << 1;
      crc = crc ^ 0xD5;
    }
    else
    {
      crc = crc << 1;
    }
  }
  return crc;
}

uint8_t CRSF_CRC_TX(uint8_t in, bool rset, bool out)
{
  static uint8_t crc;
  if (rset)
  {
     crc = 0x00;
  }
  if (out) return crc;
  crc = crc ^ in;
  for(uint8_t j = 0; j < 8; j++)
  {
    if (crc & 0b10000000)
    {
      crc = crc << 1;
      crc = crc ^ 0xD5;
    }
    else
    {
      crc = crc << 1;
    }
  }
  return crc;
}
